/*
 * Map.cpp
 *
 *  Created on: 23.11.2011
 *      Author: jan
 */

#include "Map.h"

namespace std {

Map::Map(int h, int w, int picHeight, int picWidth) : AbstractMap(picHeight,picWidth,picHeight,picWidth),update_cnt(0),/*data(),img(NULL), */imageShown(false),wDiff(picWidth / w),hDiff(picHeight / h),explored(0,0),exploreXY(0,0),first(true),min(0,0),max(0,0),measure(NULL),firstPoint(true){
	this->data.resize(picWidth,vector<DoubleProb>());
	for(int j = 0; j < picWidth; j++){
		this->data[j].resize(picHeight,DoubleProb());
	}
	cnt = 0;
}

Map::~Map() {
}

void Map::releaseImage(){
	if(imageShown){
		cvReleaseImage(&img);
		cvDestroyWindow("Map");
		cvReleaseImage(&probImg);
		cvDestroyWindow("ProbMap");
	}
}

Point Map::random_place(){
	return Point((int)(min.x + UNIFORM_RAND_WITHOUT_ONE*(max.x-min.x)),(int)(min.y + UNIFORM_RAND_WITHOUT_ONE*(max.y-min.y)));
}

Point Map::random_free_place(){
	while(true){
		Point p = random_place();
		if(this->is_free(p)){
			return Point(p);
		}
	}
	return Point(-1,-1);
}

void Map::draw(){
	imageShown = true;
	/*********************************/
	img = cvCreateImage(cvSize(picWidth, picHeight), IPL_DEPTH_8U, 3);
	probImg = cvCreateImage(cvSize(picWidth, picHeight), IPL_DEPTH_8U, 1);
	if(img == NULL || probImg == NULL){cout << "img == null" <<endl; return;}
	cvNamedWindow("Map", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("ProbMap", CV_WINDOW_AUTOSIZE);
	char *ptrD = img->imageData;
	char *ptrP = probImg->imageData;
	/*********************************/
	for (int y = 0; y < picHeight; y++) {
		for (int x = 0; x < picWidth; x++) {
			*(ptrD++) = 0xFF;
			*(ptrD++) = 0xFF;
			*(ptrD++) = 0xFF;

			*(ptrP++) = 0xFF;
		}
	}
	cvShowImage("Map" , img);
	cvShowImage("ProbMap" , probImg);
	cvWaitKey(80);
}

void Map::redraw(){
	if(img->imageData == NULL) cout << "error! AAAARGH!"<<endl;
	char *ptrD = img->imageData;
	char *ptrP = probImg->imageData;
	int value = 0;
	/*********************************/
	for (int y = 0; y < picHeight; y++) {
		for (int x = 0; x < picWidth; x++) {
			if (data[x][(picWidth - y) - 1].getValue() == -1) {
				*(ptrD++) = 0x70;
				*(ptrD++) = 0x70;
				*(ptrD++) = 0x70;
			} else if (data[x][(picWidth - y) - 1] > 0.5) {
				*(ptrD++) = 0x00;
				*(ptrD++) = 0x00;
				*(ptrD++) = 0x00;
			} else {
				*(ptrD++) = 0xFF;
				*(ptrD++) = 0xFF;
				*(ptrD++) = 0xFF;
			}
			value = static_cast<int>(data[x][(picWidth - y) - 1].getValue() * 255);
			if(value == -255) value = 0x7F;
			*(ptrP++) = value;
		}
	}
}

void Map::drawRobot(Point r){
	if( r.x >=0 && r.y >= 0 && r.x < height && r.y < width ){
		cvCircle(img, cvPoint(r.x * wDiff, picHeight - r.y * hDiff - 1), 3, cvScalar(0xFF, 0, 0), 3);
		cvCircle(img, cvPoint(r.x * wDiff, picHeight - r.y * hDiff - 1), 3, cvScalar(0, 0, 0xFF), 3);
	}
}

void Map::drawRobot(Point r, Point d){
	if( r.x >=0 && r.y >= 0 && r.x < height && r.y < width ){
		cvCircle(img, cvPoint(r.x * wDiff, picHeight - r.y * hDiff - 1), 3, cvScalar(0xFF, 0, 0), 3);
		cvCircle(img, cvPoint((r.x+3*d.x) * wDiff, picHeight - (r.y+3*d.y) * hDiff - 1), 3, cvScalar(0, 0, 0xFF), 2);
		for(unsigned int i = 0; i < measure->points.size(); i++){
			cvCircle(img, cvPoint(r.x*wDiff+measure->points[i].x*50, picHeight - (r.y*hDiff+measure->points[i].y*50) - 1), 1, cvScalar(0xFF, 0xFF, 0), 0);
		}
	}
}

void Map::drawParticles(Point p){
	if(p.x >=0 && p.y >= 0 && p.x < height && p.y < width ){
		cvCircle(img, cvPoint(p.x, picHeight - p.y - 1), 2, cvScalar(0, 0xFF, 0), 0);
		for(unsigned int i = 0; i < measure->points.size(); i++){
			cvCircle(img, cvPoint(p.x+measure->points[i].x*50, picHeight - (p.y+measure->points[i].y*50) - 1), 1, cvScalar(0xFF, 0, 0xFF), 0);
		}
	}
}

void Map::setPoint(Point p){
	data[p.x][p.y]++;
}

void Map::clearPoint(Point p){
	data[p.x][p.y]--;
}

void Map::drawLine(Point start, Point end){
	drawLine(start,end,false);
}
void Map::drawLine(Point start, Point end,bool wall){
	int startX = static_cast<int>(start.x);
	int startY = static_cast<int>(start.y);
	int endX = static_cast<int>(end.x);
	int endY = static_cast<int>(end.y);
	int dx = abs(endX - startX);
	int sx = startX < endX ? 1 : -1;
	int dy = -abs(endY - startY);
	int sy = startY < endY ? 1 : -1;
	int x = startX, y = startY;
	int err = dx+dy;
	int e2 = 0;
	while(!(endX == x && endY == y) ){
		if(wall)
			setPoint(Point(x,y));
		else
			clearPoint(Point(x,y));
		e2 = err*2;
		if(e2 > dy){
			err +=dy;
			x += sx;
		}
		if(e2 < dx){
			err +=dx;
			y += sy;
		}
	}
	if (wall) {
		setPoint(Point(x, y));
	} else {
		clearPoint(Point(x, y));
	}
}


void Map::repaint(){
	firstPoint = true;
	cvShowImage("Map", img);
	cvShowImage("ProbMap", probImg);
	cvWaitKey(10);
}

#ifdef BLA
void Map::updateMap(vector<double> scan, int grad, double heading, Point p){
	int degree = -90;
	for(unsigned int i = 0; i < scan.size(); i++, degree+=grad){
		for(double radius = 0; radius <= 0.5; radius+=0.1){
			double r = radians((heading+degree));
			double xk = radius * cos(r);
			double yk = radius * sin(r);
			Point l (p);
			l.x = l.x+xk;
			l.y = l.y+yk;
			if(l.x < 0) l.x = 0;
			if(l.y < 0) l.y = 0;
			if(l.x >= 10) l.x = 9.98;
			if(l.y >= 10) l.y = 9.98;
			if(radius <= scan[i]){
				data[l.x*50][l.y*50]--;
			}else{
				data[l.x*50][l.y*50]++;
			}
		}
	}
}
#else
void Map::updateMap(Measurement m){//){
	int degree = -90;
	Point start(m.p.x*50,m.p.y*50);
	if(first){
		min.x = max.x = (int) start.x;
		min.y = max.y = (int) start.y;
		first = false;
	}else{
		explore(start);
	}
	for(unsigned int i = 0; i < m.scan.size(); i++, degree+=m.grad){
		double r = radians((m.heading+degree));
		double xk = m.scan[i] * cos(r);
		double yk = m.scan[i] * sin(r);
		Point l(m.p.x+xk,m.p.y+yk);
		if (l.x < 0) l.x = 0;
		if (l.y < 0) l.y = 0;
		if (l.x >= 10) l.x = 9.98;
		if (l.y >= 10) l.y = 9.98;
		l.x = l.x*50;l.y = l.y*50;
		drawLine(start, l);
		if(m.scan[i] < 0.5){
			xk = 0.5 * cos(r);
			yk = 0.5 * sin(r);
			Point q (l);
			l.x = m.p.x+xk;
			l.y = m.p.y+yk;
			if (l.x < 0) l.x = 0;
			if (l.y < 0) l.y = 0;
			if (l.x >= 10) l.x = 9.98;
			if (l.y >= 10) l.y = 9.98;
			l.x = l.x*50;l.y = l.y*50;
			drawLine(q,l,true);
		}
		explore(l);
	}
	if(this->measure != NULL) delete this->measure;
	this->measure = new Measurement(m);
	mapvalue = 0;
	for(unsigned int i = 0; i < this->measure->points.size(); i++){
		mapvalue +=(data[(this->measure->points[i].x+this->measure->p.x)*50][(this->measure->points[i].y+this->measure->p.y)*50]).getValue();
	}
}
#endif
void Map::explore(Point p){
	if(p.x > max.x) max.x = p.x;
	if(p.y > max.y) max.y = p.y;
	if(p.x < min.x) min.x = p.x;
	if(p.y < min.y) min.y = p.y;
}

bool Map::is_in(Point p){
	return ((!(p.x < 0 || p.y < 0 || p.x > this->width || p.y > this->height)) && (data[(int)p.x][(int)p.y].getValue() != -1));
}

double Map::distance(Point p1, Point p2){
	return sqrt((p1.x - p2.x)*(p1.x - p2.x)+(p1.y - p2.y)*(p1.y - p2.y));
}

double Map::distance_to_nearest_beacon(Point p){
	double d = 9999;
	return d;
}

bool Map::is_free(Point p){
	if(!this->is_in(p)) return false;
	int x_co = (int)p.x;
	int y_co = (int)p.y;//height - (int)p.y - 1;
	return data[x_co][y_co].getValue() != -1 && data[x_co][y_co].getValue()<0.5;
}

} /* namespace std */
